#include "mpcommon.h"

enum libusb_error send_feature_report(libusb_device_handle *devh, unsigned char *buffer, unsigned int length, unsigned int *transferred) {
    bool kernel_driver = false;
    int transfer = 0;
    int result = 0;

    if (libusb_kernel_driver_active(devh, INTERFACE_NUMBER) == 1) {
	kernel_driver = true;
	result = libusb_detach_kernel_driver(devh, INTERFACE_NUMBER);
	if (result != LIBUSB_SUCCESS) {
	    return result;
	}
    }

    result = libusb_claim_interface(devh, INTERFACE_NUMBER);

    if (result == LIBUSB_SUCCESS) {
	transfer = libusb_control_transfer(devh,
				    CONTROL_REQUEST_TYPE_OUT,
				    HID_SET_REPORT,
				    (HID_REPORT_TYPE_FEATURE << 8) | 0x00,
				    INTERFACE_NUMBER,
				    buffer,
				    length,
				    TIMEOUT_MS);

	result = libusb_release_interface(devh, INTERFACE_NUMBER);
    }

    if (transfer >= 0) {
	*transferred = transfer;
	return LIBUSB_SUCCESS;
    } else {
	return transfer;
    }
}

enum libusb_error receive_feature_report(libusb_device_handle *devh, unsigned char *buffer, unsigned int length, unsigned int *transferred) {
    bool kernel_driver = false;
    int transfer = 0;
    int result = 0;

    if (libusb_kernel_driver_active(devh, INTERFACE_NUMBER) == 1) {
	kernel_driver = true;
	result = libusb_detach_kernel_driver(devh, INTERFACE_NUMBER);
	if (result != LIBUSB_SUCCESS) {
	    return result;
	}
    }

    result = libusb_claim_interface(devh, INTERFACE_NUMBER);

    if (result == LIBUSB_SUCCESS) {
	transfer = libusb_control_transfer(devh,
				    CONTROL_REQUEST_TYPE_IN,
				    HID_GET_REPORT,
				    (HID_REPORT_TYPE_FEATURE << 8) | 0x00,
				    INTERFACE_NUMBER,
				    buffer,
				    length,
				    TIMEOUT_MS);

	result = libusb_release_interface(devh, INTERFACE_NUMBER);
    }

    if (transfer >= 0) {
	*transferred = transfer;
	return LIBUSB_SUCCESS;
    } else {
	return transfer;
    }
}

enum libusb_error exchange_feature_reports_dev_id(libusb_device_handle *devh, unsigned int *id, unsigned char *family) {
    int transferred = 0;
    unsigned char in[8];
    unsigned char out[8];
    int i = 0;
    enum libusb_error result = LIBUSB_SUCCESS;

    for (i=0; i < 8; i++) {
	out[i]=0x00;
	in[i]=0x00;
    }

    out[0] = 0x1D;
    out[1] = 0x00;
    out[2] = 0x00;
    out[3] = 0x00;
    out[4] = 0x00;
    out[5] = 0x00;
    out[6] = 0x00;
    out[7] = 0x00;

    if (result = send_feature_report(devh, out, sizeof(out), &transferred) == LIBUSB_SUCCESS && transferred > 0) {
	if (result = receive_feature_report(devh, in, sizeof(in), &transferred) == LIBUSB_SUCCESS && transferred > 0) {
	    *id = (in[4]<<24) + (in[5]<<16) + (in[6]<<8) + in[7];
	    *family = in[1];
	}
    }

    return result;
}

enum libusb_error lookup_device(libusb_context *ctx, struct libusb_device_handle **devh, unsigned int id, unsigned char family) {
    libusb_device			**devices = NULL;
    enum libusb_error			result;
    
    int count = libusb_get_device_list(ctx, &devices);
	
    if (count > 0) {
	for (count--; count >= 0; count--) {
	    struct libusb_device_descriptor *desc = calloc(1, sizeof(struct libusb_device_descriptor));
	    result = libusb_get_device_descriptor(devices[count], desc);
	    if (result ==  LIBUSB_SUCCESS) {
		if (desc->idVendor == VENDOR_ID && desc->idProduct == PRODUCT_ID) {
		    free(desc);
		    result = libusb_open(devices[count], devh);
		    if (result == LIBUSB_SUCCESS) {
			unsigned int candidate_id;
			unsigned char candidate_family;
			if (id == 0 && (family == 0 || candidate_family == family)) {
			    break;
			}
			result = exchange_feature_reports_dev_id(*devh, &candidate_id, &candidate_family);
			if (result == LIBUSB_SUCCESS || result > 0) {
			    if (id == candidate_id && candidate_family == family) {
				result = LIBUSB_SUCCESS;
				break;
			    }
			} else {
			    libusb_reset_device(*devh);
			    break;
			}
		    }
		    libusb_close(*devh);
		    *devh = NULL;
		}
	    }
	}
    }

    libusb_free_device_list(devices, true);
    return result;
}
